# -- coding: utf-8 --
"""
# 图像和压缩图像数据帧消息的 FastDDS 发布器/订阅器

**模块：MIAHX© / 框架基础服务层（FFST） / 交互框架信息表示模型（IFIRM）**

Version: 0.4.9,
Last updated: August 28, 2024

## Contributor(s)
* YAO Naiming <[naiming@iscas.ac.cn](mailto:naiming@iscas.ac.cn)>

## Change log
### v0.4.9
- .28: [新增] 初始实现

---
Copyright © 2024-2025 MIAHX Contributors, ISCAS, and ASIC. All Rights Reserved.
Licensed under Apache-2.0.
"""

import threading
import time

from miahx_support.dds.fastdds import *
from miahx_support.stag import ESensorType

import launcher


# **** BEAMMP SERVER STARTUP ========================================


def write_beammp_server_startup_msg(p: Participant):
    log.info('Starting BeamMP Server ...')
    time.sleep(1)
    writer = BeamMPServerStartupMsgWriter(p)
    writer.write(name='China Hefei', map_path='/levels/smallgrid/info.json',
                 auth_key='02e8576e-05b1-4fac-9266-82c27e6f6b9a', description='China Hefei Map')
    log.success('START `BeamMP Server` ...')
    time.sleep(1)
    log.success('BeamMP Server started!')


def read_beammp_server_startup_msg(p: Participant, e: threading.Event):
    def on_beammp_server_startup_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] name = `{data.name()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] map_path = `{data.map_path()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] description = `{data.description()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] port = `{data.port()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] non_public = `{data.non_public()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] auth_key = `{data.auth_key()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] tags = `{data.tags()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] max_cars = `{data.max_cars()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] max_players = `{data.max_players()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] log_chat = `{data.log_chat()}`')
        log.info(f'[{domain}::{source}::BeamMPServerStartupMsgReader] enable_debug = `{data.enable_debug()}`')

    with BeamMPServerStartupMsgReader(p, on_data_listeners=[on_beammp_server_startup_msg_available]) as reader:
        log.success('[BeamMPServerStartupMsgReader] Waiting data ...')
        reader.wait(e)


# **** BEAMNG STARTUP ========================================


def write_beamng_startup_msg(p: Participant):
    log.info('Starting BeamNG ...')
    time.sleep(1)
    writer = BeamNGStartupMsgWriter(p)
    vehicles = [dict(vid='PV_ID', model='etk800', licence='111', color='white',
                     spawn_pos=(6470.574, 2768.198, 125.134), spawn_rot=(0, 0, 90))]
    writer.write('SUBJECT', '11:05', 10.0, 1.0, 200,
                 2, 30, 'Random', 0.2, 100, 20,
                 vehicles)
    log.success('START `BeamNG` ...')
    time.sleep(1)
    log.success('BeamNG started!')


def read_beamng_startup_msg(p: Participant, e: threading.Event):
    def on_beamng_startup_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] name = `{data.name()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] time_of_day = `{data.env_tod()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] fog_density = `{data.env_fog_density()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] rain_nums = `{data.env_rain_nums()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] water_height = `{data.env_rain_water_plane_height()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] traffic_density = `{data.traffic_density()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] traffic_speed_limit = `{data.traffic_speed_limit()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] traffic_mode = `{data.traffic_mode()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] traffic_aggression = `{data.traffic_aggression()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] traffic_spawn_freq = `{data.traffic_spawn_freq()}`')
        log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] traffic_gap = `{data.traffic_gap()}`')
        for i, item in enumerate(data.vehicles()):
            log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] VEHICLE #{i + 1}')
            log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] \tvid = `{item.vid()}`')
            log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] \tmodel = `{item.model()}`')
            log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] \tlicence = `{item.licence()}`')
            log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] \tcolor = `{item.color()}`')
            log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] \tspawn_pos = `{tuple(item.spawn_pos())}`')
            log.info(f'[{domain}::{source}::BeamNGStartupMsgReader] \tspawn_rot = `{tuple(item.spawn_rot())}`')

    with BeamNGStartupMsgReader(p, on_data_listeners=[on_beamng_startup_msg_available]) as reader:
        log.success('[BeamNGStartupMsgReader] Waiting data ...')
        reader.wait(e)


# **** HUD STARTUP ========================================


def write_hud_startup_msg(p: Participant):
    log.info('Starting HUD ...')
    time.sleep(1)
    writer = HudStartupMsgWriter(p)
    writer.write()
    log.success('START `HUD` ...')
    time.sleep(1)
    log.success('HUD started!')


def read_hud_startup_msg(p: Participant, e: threading.Event):
    def on_hud_startup_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::HudStartupMsgReader] {data.name()}`')

    with HudStartupMsgReader(p, on_data_listeners=[on_hud_startup_msg_available]) as reader:
        log.success('[HudStartupMsgReader] Waiting data ...')
        reader.wait(e)


# **** GNSS SYNC STARTUP ========================================


def write_gnss_sync_startup_msg(p: Participant):
    log.info('Starting GNSS Sync ...')
    time.sleep(1)
    writer = GNSSyncStartupMsgWriter(p)
    writer.write(period=1, use_gnss=True)
    log.success('START `GNSS Sync` ...')
    time.sleep(1)
    log.success('GNSS Sync started!')


def read_gnss_sync_startup_msg(p: Participant, e: threading.Event):
    def on_gnss_sync_startup_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::GNSSyncStartupMsgReader] period = `{data.period()}`')
        log.info(f'[{domain}::{source}::GNSSyncStartupMsgReader] use_gnss = `{data.use_gnss()}`')

    with GNSSyncStartupMsgReader(p, on_data_listeners=[on_gnss_sync_startup_msg_available]) as reader:
        log.success('[GNSSyncStartupMsgReader] Waiting data ...')
        reader.wait(e)


# **** DATA COLLECTOR STARTUP ========================================


def write_data_collector_startup_msg(p: Participant):
    def make_pvcc_basler_profile():
        return dict(name='PVCC_BASLER', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PVCC_BASLER.name,
                    profile=dict(mode='poll', dev_id=1, resolution=(8, 6), preview_quality=95))

    def make_pvgc_smart_eye_profile():
        return dict(name='PVGC_SMART_EYE', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PVGC_SMART_EYE.name,
                    profile=dict(mode='poll', dev_host='127.0.0.1', dev_port=8888, dev_name='SMART_EYE',
                                 srate=256, time_buffer=6))

    def make_pvgc_smart_eye_snapshot_profile():
        return dict(name='PVGC_SMART_EYE_SNAPSHOT', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PVGC_SMART_EYE_SNAPSHOT.name,
                    profile=dict(mode='poll', screen_size=(2, 2), snapshot_size=(3, 3), snapshot_desired_size=(4, 4),
                                 positions=[1, 2, 3, 4, 5, 6], n_channels=3, time_buffer=6))

    def make_pamc_dji_profile():
        return dict(name='PAMC_DJI', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PAMC_DJI.name,
                    profile=dict(mode='poll', sample_width=2,
                                 frame_rate=16000, n_channels=2, time_buffer=6))

    def make_peeg_neuracle_profile():
        return dict(name='PEEG_NEURACLE', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PEEG_NEURACLE.name,
                    profile=dict(mode='poll', dev_host='127.0.0.1', dev_port=8888, dev_name='DSI-24',
                                 srate=256, n_channels=4, time_buffer=6))

    def make_pwbd_psych_tech_gsr_profile():
        return dict(name='PWBD_PSYCH_TECH_GSR', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PWBD_PSYCH_TECH_GSR.name,
                    profile=dict(mode='poll', dev_host='127.0.0.1', dev_port=8888,
                                 points=100, n_channels=4))

    def make_pwbd_psych_tech_ppg_profile():
        return dict(name='PWBD_PSYCH_TECH_PPG', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PWBD_PSYCH_TECH_PPG.name,
                    profile=dict(mode='poll', dev_host='127.0.0.1', dev_port=8888,
                                 points=100, n_channels=100))

    def make_pphs_physiolab_profile():
        return dict(name='PPHS_PHYSIOLAB', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.PPHS_PHYSIOLAB.name,
                    profile=dict(mode='poll', dev_host='127.0.0.1', dev_port=8888,
                                 points=100, channels=['ch1', 'ch2']))

    def make_bng_camera_profile():
        return dict(name='VBNG_CAMERA', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.VBNG_CAMERA.name,
                    profile=dict(general=dict(visible=True, pos=(1, 2, 3), dir=(4, 5, 6)),
                                 resolution=(640, 480), quality=95, use_shm=True, fov_y=60,
                                 nfp=(0.1, 100), allow_depth=True, allow_annotation=True, allow_instance=True))

    def make_vbng_lidar_profile():
        return dict(name='VBNG_LIDAR', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.VBNG_LIDAR.name,
                    profile=dict(general=dict(visible=True, pos=(1, 2, 3), dir=(4, 5, 6)),
                                 use_shm=True, is_annotated=True, rays_per_second=2200,
                                 frequency=20, max_distance=120))

    def make_vbng_radar_profile():
        return dict(name='VBNG_RADAR', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.VBNG_RADAR.name,
                    profile=dict(general=dict(visible=True, pos=(1, 2, 3), dir=(4, 5, 6)),
                                 use_shm=True, resolution=(200, 200), fov_y=60, nfp=(0.1, 150)))

    def make_vbng_classical_profile():
        return dict(name='VBNG_CLASSICAL', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.VBNG_CLASSICAL.name,
                    profile=dict(general=dict(visible=True, pos=(1, 2, 3), dir=(4, 5, 6)),
                                 allow_timer=True, allow_ecu=True, allow_imu=True,
                                 allow_damage=True, allow_gforces=True, ))

    def make_bng_gps_profile():
        return dict(name='VBNG_GPS', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.VBNG_GPS.name,
                    profile=dict(general=dict(visible=True, pos=(1, 2, 3), dir=(4, 5, 6)),
                                 offsets=(7, 8)))

    def make_vbng_imu_adv_profile():
        return dict(name='VBNG_IMU_ADV', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.VBNG_IMU_ADV.name,
                    profile=dict(general=dict(visible=True, pos=(1, 2, 3), dir=(4, 5, 6))))

    def make_vbng_navigation_profile():
        return dict(name='VBNG_NAVIGATION', sensor_type=1, channel_type=2, channel_id=3, modality=4,
                    data_format=5, fps=6, allow_store=True, allow_publish=True,
                    profile_name=ESensorType.VBNG_NAVIGATION.name,
                    profile=dict(general=dict(visible=True, pos=(1, 2, 3), dir=(4, 5, 6)),
                                 ai_speed_limit=50.0))

    log.info('Starting Data Collector ...')
    time.sleep(1)
    writer = DataCollectorStartupMsgWriter(p)
    profiles = [
        make_pvcc_basler_profile(),
        make_pvgc_smart_eye_profile(),
        make_pvgc_smart_eye_snapshot_profile(),
        make_pamc_dji_profile(),
        make_peeg_neuracle_profile(),
        make_pwbd_psych_tech_gsr_profile(),
        make_pwbd_psych_tech_ppg_profile(),
        make_pphs_physiolab_profile(),
        make_bng_camera_profile(),
        make_vbng_lidar_profile(),
        make_vbng_radar_profile(),
        make_vbng_classical_profile(),
        make_bng_gps_profile(),
        make_vbng_imu_adv_profile(),
        make_vbng_navigation_profile()
    ]
    writer.write(name='BNG_CARRIER', group='beamng', target_vid='EGO',
                 record_id='M001', record_tag='TAG', profiles=profiles)
    log.success('START `Data Collector` ...')
    time.sleep(1)
    log.success('Data Collector started!')


def read_data_collector_startup_msg(p: Participant, e: threading.Event):
    def log_bng_sensor_general_profile(domain, source, sp):
        # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] vbng.visible = `{sp.general().visible()}`')
        # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] vbng.pos = `{tuple(sp.general().pos())}`')
        # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] vbng.dir = `{tuple(sp.general().dir())}`')
        pass

    def log_pvcc_basler_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cam.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cam.dev_id = `{sp.dev_id()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cam.resolution = `{tuple(sp.resolution())}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cam.preview_quality = `{sp.preview_quality()}`')

    def log_pvgc_smart_eye_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.dev_name = `{sp.dev_name()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.dev_host = `{sp.dev_host()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.dev_port = `{sp.dev_port()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.srate = `{sp.srate()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.time_buffer = `{sp.time_buffer()}`')

    def log_pvgc_smart_eye_snapshot_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.screen_size = `{tuple(sp.screen_size())}`')
        log.info(
            f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.snapshot_size = `{tuple(sp.snapshot_size())}`')
        log.info(
            f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.snapshot_desired_size = `{tuple(sp.snapshot_desired_size())}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.positions = `{tuple(sp.positions())}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.n_channels = `{sp.n_channels()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] eye.time_buffer = `{sp.time_buffer()}`')

    def log_pamc_dji_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] mic.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] mic.n_channels = `{sp.n_channels()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] mic.sample_width = `{sp.sample_width()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] mic.frame_rate = `{sp.frame_rate()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] mic.time_buffer = `{sp.time_buffer()}`')

    def log_peeg_neuracle_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] dsi.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] dsi.dev_name = `{sp.dev_name()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] dsi.dev_host = `{sp.dev_host()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] dsi.dev_port = `{sp.dev_port()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] dsi.srate = `{sp.srate()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] dsi.n_channels = `{sp.n_channels()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] dsi.time_buffer = `{sp.time_buffer()}`')

    def log_pwbd_psych_tech_gsr_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] gsr.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] gsr.dev_host = `{sp.dev_host()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] gsr.dev_port = `{sp.dev_port()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] gsr.points = `{sp.points()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] gsr.channels = `{sp.n_channels()}`')

    def log_pwbd_psych_tech_ppg_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] ppg.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] ppg.dev_host = `{sp.dev_host()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] ppg.dev_port = `{sp.dev_port()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] ppg.points = `{sp.points()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] ppg.channels = `{sp.n_channels()}`')

    def log_pphs_physiolab_profile(domain, source, sp):
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] physio.mode = `{sp.mode()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] physio.dev_host = `{sp.dev_host()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] physio.dev_port = `{sp.dev_port()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] physio.points = `{sp.points()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] physio.channels = `{list(sp.channels())}`')

    def log_bng_camera_profile(domain, source, sp):
        log_bng_sensor_general_profile(domain, source, sp)
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.resolution = `{tuple(sp.resolution())}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.quality = `{sp.quality()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.use_shm = `{sp.use_shm()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.fov_y = `{sp.fov_y()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.nfp = `{tuple(sp.nfp())}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.allow_depth = `{sp.allow_depth()}`')
        log.info(
            f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.allow_annotation = `{sp.allow_annotation()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] camera.allow_instance = `{sp.allow_instance()}`')

    def log_bng_lidar_profile(domain, source, sp):
        log_bng_sensor_general_profile(domain, source, sp)
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] lidar.use_shm = `{sp.use_shm()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] lidar.is_annotated = `{sp.is_annotated()}`')
        log.info(
            f'[{domain}::{source}::DataCollectorStartupMsgReader] lidar.rays_per_second = `{sp.rays_per_second()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] lidar.frequency = `{sp.frequency()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] lidar.max_distance = `{sp.max_distance()}`')

    def log_bng_radar_profile(domain, source, sp):
        log_bng_sensor_general_profile(domain, source, sp)
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] radar.use_shm = `{sp.use_shm()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] radar.resolution = `{tuple(sp.resolution())}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] radar.fov_y = `{sp.fov_y()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] radar.nfp = `{tuple(sp.nfp())}`')

    def log_bng_classical_profile(domain, source, sp):
        log_bng_sensor_general_profile(domain, source, sp)
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cls.allow_timer = `{sp.allow_timer()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cls.allow_timer = `{sp.allow_ecu()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cls.allow_timer = `{sp.allow_imu()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cls.allow_timer = `{sp.allow_damage()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] cls.allow_timer = `{sp.allow_gforces()}`')

    def log_bng_gps_profile(domain, source, sp):
        log_bng_sensor_general_profile(domain, source, sp)
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] gps.offsets = `{tuple(sp.offsets())}`')

    def log_vbng_imu_adv_profile(domain, source, sp):
        log_bng_sensor_general_profile(domain, source, sp)

    def log_vbng_navigation_profile(domain, source, sp):
        log_bng_sensor_general_profile(domain, source, sp)
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] navi.ai_speed_limit = `{sp.ai_speed_limit()}`')

    def on_data_collector_startup_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] name = `{data.name()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] group = `{data.group()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] target_vid = `{data.target_vid()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] record_id = `{data.record_id()}`')
        log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] record_tag = `{data.record_tag()}`')
        for item in data.sensors():
            log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] name = `{item.name()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] domain = `{item.domain()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] sensor_type = `{item.sensor_type()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] channel_type = `{item.channel_type()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] channel_id = `{item.channel_id()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] modality = `{item.modality()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] data_format = `{item.data_format()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] fps = `{item.fps()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] allow_publish = `{item.allow_publish()}`')
            # log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] allow_store = `{item.allow_store()}`')
            profile = getattr(item.profile(), item.profile_name())()
            loggers.get(ESensorType[item.profile_name().upper()])(domain, source, profile)
            log.info(f'[{domain}::{source}::DataCollectorStartupMsgReader] ==============================')

    loggers = {
        ESensorType.PVCC_BASLER: log_pvcc_basler_profile,
        ESensorType.PVGC_SMART_EYE: log_pvgc_smart_eye_profile,
        ESensorType.PVGC_SMART_EYE_SNAPSHOT: log_pvgc_smart_eye_snapshot_profile,
        ESensorType.PAMC_DJI: log_pamc_dji_profile,
        ESensorType.PEEG_NEURACLE: log_peeg_neuracle_profile,
        ESensorType.PWBD_PSYCH_TECH_GSR: log_pwbd_psych_tech_gsr_profile,
        ESensorType.PWBD_PSYCH_TECH_PPG: log_pwbd_psych_tech_ppg_profile,
        ESensorType.PPHS_PHYSIOLAB: log_pphs_physiolab_profile,
        ESensorType.VBNG_CAMERA: log_bng_camera_profile,
        ESensorType.VBNG_LIDAR: log_bng_lidar_profile,
        ESensorType.VBNG_RADAR: log_bng_radar_profile,
        ESensorType.VBNG_CLASSICAL: log_bng_classical_profile,
        ESensorType.VBNG_GPS: log_bng_gps_profile,
        ESensorType.VBNG_IMU_ADV: log_vbng_imu_adv_profile,
        ESensorType.VBNG_NAVIGATION: log_vbng_navigation_profile
    }
    with DataCollectorStartupMsgReader(p, on_data_listeners=[on_data_collector_startup_msg_available]) as reader:
        log.success('[DataCollectorStartupMsgReader] Waiting data ...')
        reader.wait(e)


# **** DATA COLLECTOR STARTUP ========================================


def write_process_startup_ack_msg(p: Participant):
    time.sleep(1)
    writer = ProcessStartupAckMsgWriter(p)
    writer.write(name='abc', pid=100)
    time.sleep(1)


def read_process_startup_ack_msg(p: Participant, e: threading.Event):
    def on_process_startup_ack_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::ProcessStartupAckMsgReader] name = `{data.name()}`')
        log.info(f'[{domain}::{source}::ProcessStartupAckMsgReader] pid = `{data.pid()}`')

    with ProcessStartupAckMsgReader(p, on_data_listeners=[on_process_startup_ack_msg_available]) as reader:
        log.success('[ProcessStartupAckMsgReader] Waiting data ...')
        reader.wait(e)


# **** PROCESS SHUTDOWN ========================================


def write_process_shutdown_msg(p: Participant):
    time.sleep(1)
    writer = ProcessShutdownMsgWriter(p)
    writer.write(name='abcde', pid=20)
    time.sleep(1)


def read_process_shutdown_msg(p: Participant, e: threading.Event):
    def on_process_shutdown_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::ProcessShutdownMsgReader] name = `{data.name()}`')
        log.info(f'[{domain}::{source}::ProcessShutdownMsgReader] pid = `{data.pid()}`')

    with ProcessShutdownMsgReader(p, on_data_listeners=[on_process_shutdown_msg_available]) as reader:
        log.success('[ProcessShutdownMsgReader] Waiting data ...')
        reader.wait(e)


# **** PROCESS SHUTDOWN ========================================


def write_process_shutdown_ack_msg(p: Participant):
    time.sleep(1)
    writer = ProcessShutdownAckMsgWriter(p)
    writer.write(name='abcd', pid=300)
    time.sleep(1)


def read_process_shutdown_ack_msg(p: Participant, e: threading.Event):
    def on_process_shutdown_ack_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::ProcessShutdownAckMsgReader] name = `{data.name()}`')
        log.info(f'[{domain}::{source}::ProcessShutdownAckMsgReader] pid = `{data.pid()}`')

    with ProcessShutdownAckMsgReader(p, on_data_listeners=[on_process_shutdown_ack_msg_available]) as reader:
        log.success('[ProcessShutdownAckMsgReader] Waiting data ...')
        reader.wait(e)


# **** PROCESS HEARTBEAT ========================================


def write_process_heartbeat_msg(p: Participant):
    time.sleep(1)
    writer = ProcessHeartbeatMsgWriter(p)
    writer.write(name='abcd', pid=300, is_running=True)
    time.sleep(1)


def read_process_heartbeat_msg(p: Participant, e: threading.Event):
    def on_process_heartbeat_msg_available(data):
        domain = data.header().domain()
        source = data.header().source()
        log.info(f'[{domain}::{source}::ProcessHeartbeatMsgReader] name = `{data.name()}`')
        log.info(f'[{domain}::{source}::ProcessHeartbeatMsgReader] pid = `{data.pid()}`')
        log.info(f'[{domain}::{source}::ProcessHeartbeatMsgReader] is_running = `{data.is_running()}`')

    with ProcessHeartbeatMsgReader(p, on_data_listeners=[on_process_heartbeat_msg_available]) as reader:
        log.success('[ProcessHeartbeatMsgReader] Waiting data ...')
        reader.wait(e)


if __name__ == '__main__':
    log = launcher.log
    event = threading.Event()
    participant = Participant()
    launcher.run(lambda: (event, [
        # threading.Thread(target=write_beammp_server_startup_msg, args=(participant,), daemon=False),
        # threading.Thread(target=read_beammp_server_startup_msg, args=(participant, event), daemon=False),
        #
        # threading.Thread(target=write_beamng_startup_msg, args=(participant,), daemon=False),
        # threading.Thread(target=read_beamng_startup_msg, args=(participant, event), daemon=False),
        #
        # threading.Thread(target=write_gnss_sync_startup_msg, args=(participant,), daemon=False),
        # threading.Thread(target=read_gnss_sync_startup_msg, args=(participant, event), daemon=False),
        #
        threading.Thread(target=write_data_collector_startup_msg, args=(participant,), daemon=False),
        threading.Thread(target=read_data_collector_startup_msg, args=(participant, event), daemon=False),
        #
        # threading.Thread(target=write_process_startup_ack_msg, args=(participant,), daemon=False),
        # threading.Thread(target=read_process_startup_ack_msg, args=(participant, event), daemon=False),
        #
        # threading.Thread(target=write_process_shutdown_msg, args=(participant,), daemon=False),
        # threading.Thread(target=read_process_shutdown_msg, args=(participant, event), daemon=False),
        #
        # threading.Thread(target=write_process_shutdown_ack_msg, args=(participant,), daemon=False),
        # threading.Thread(target=read_process_shutdown_ack_msg, args=(participant, event), daemon=False),
        #
        # threading.Thread(target=write_process_heartbeat_msg, args=(participant,), daemon=False),
        # threading.Thread(target=read_process_heartbeat_msg, args=(participant, event), daemon=False),
    ]))
